﻿using System;
using NPack.Interfaces;

namespace NPack
{
    /// <summary>
    /// Eigenvalues and eigenvectors of a real matrix.
    /// <para/>
    /// If A is symmetric, then A = V*D*V' where the eigenvalue matrix D is
    /// diagonal and the eigenvector matrix V is orthogonal.
    /// I.e. A = V.times(D.times(V.transpose())) and
    /// V.times(V.transpose()) equals the identity matrix.
    /// <para/>
    /// If A is not symmetric, then the eigenvalue matrix D is block diagonal
    /// with the real eigenvalues in 1-by-1 blocks and any complex eigenvalues,
    /// lambda + i*mu, in 2-by-2 blocks, [lambda, mu; -mu, lambda].  The
    /// columns of V represent the eigenvectors in the sense that A*V = V*D,
    /// i.e. A.times(V) equals V.times(D).  The matrix V may be badly
    /// conditioned, or even singular, so the validity of the equation
    /// A = V*D*inverse(V) depends upon V.cond().
    /// </summary>
    /// <typeparam name="TComponent">The type of the matrix' components.</typeparam>
    /// <typeparam name="TMatrix">The type of the matrix</typeparam>
    public class EigenvalueDecomposition<TComponent, TMatrix> : IEigenvalueDecomposition<TComponent, TMatrix>
        where TComponent : struct, IEquatable<TComponent>, IComparable<TComponent>, IComputable<TComponent>, IConvertible, IFormattable
        where TMatrix : IMatrix<TComponent, TMatrix>
    {
        private static readonly TComponent Zero = default(TComponent).Zero;
        private static readonly TComponent One = default(TComponent).One;
        private static readonly TComponent Two = One.Add(One);
        private static readonly TComponent Eps = Two.Power(-52.0);

        private readonly IMatrixFactory<TComponent, TMatrix> _matrixFactory;
        private readonly TMatrix _inputMatrix;

        /* ------------------------
           Class variables
         * ------------------------ */

        /*
         * Row and column dimension (square matrix).
         */
        private readonly int _n;

        /*
         * Symmetry flag.
        */
        private readonly bool _isSymmetric;

        /*
         * Arrays for internal storage of eigenvalues.
         */
        private readonly TComponent[] _d;
        private readonly TComponent[] _e;

        /*
         * Array for internal storage of eigenvectors.
         */
        private readonly TMatrix _v;

        /* ------------------------
           Private Methods
         * ------------------------ */

        // Symmetric Householder reduction to tridiagonal form.
        private void Tred2()
        {
            //  This is derived from the Algol procedures tred2 by
            //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
            //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
            //  Fortran subroutine in EISPACK.

            for (var j = 0; j < _n; j++)
            {
                _d[j] = _v[_n - 1,j];
            }

            // Householder reduction to tridiagonal form.

            for (var i = _n - 1; i > 0; i--)
            {
                // Scale to avoid under/overflow.
                var scale = Zero;
                var h = Zero;
                for (var k = 0; k < i; k++)
                {
                    scale = scale.Add(_d[k].Abs());
                }
                if (scale.Equals(Zero))
                {
                    _e[i] = _d[i - 1];
                    for (var j = 0; j < i; j++)
                    {
                        _d[j] = _v[i - 1, j];
                        _v[i, j] = Zero;
                        _v[j, i] = Zero;
                    }
                }
                else
                {
                    // Generate Householder vector.
                    for (var k = 0; k < i; k++)
                    {
                        _d[k] = _d[k].Divide(scale);
                        h = h.Add(_d[k].Multiply(_d[k]));
                    }
                    var f = _d[i - 1];
                    var g = h.Sqrt();
                    if (f.GreaterThan(Zero))
                    {
                        g = g.Negative();
                    }
                    _e[i] = scale.Multiply(g);
                    h = h.Subtract(f.Multiply(g));
                    _d[i - 1] = f.Subtract(g);
                    for (var j = 0; j < i; j++)
                    {
                        _e[j] = Zero;
                    }

                    // Apply similarity transformation to remaining columns.
                    for (var j = 0; j < i; j++)
                    {
                        f = _d[j];
                        _v[j, i] = f;
                        g = _e[j].Add(_v[j, j].Multiply(f));
                        for (var k = j + 1; k <= i - 1; k++)
                        {
                            g = g.Add(_v[k, j].Multiply(_d[k]));
                            _e[k] = _e[k].Add(_v[k, j].Multiply(f));
                        }
                        _e[j] = g;
                    }
                    f = Zero;
                    for (var j = 0; j < i; j++)
                    {
                        _e[j] = _e[j].Divide(h);
                        f = f.Add(_e[j].Multiply(_d[j]));
                    }
                    var hh = f.Divide(h.Add(h));
                    for (var j = 0; j < i; j++)
                    {
                        _e[j] = _e[j].Add(hh.Multiply(_d[j]));
                    }
                    for (var j = 0; j < i; j++)
                    {
                        f = _d[j];
                        g = _e[j];
                        for (var k = j; k <= i - 1; k++)
                        {
                            _v[k, j] = _v[k, j].Subtract((f.Multiply(_e[k]).Add(g.Multiply(_d[k]))));
                        }
                        _d[j] = _v[i - 1, j];
                        _v[i, j] = Zero;
                    }
                }
                _d[i] = h;
            }

            // Accumulate transformations.

            for (var i = 0; i < _n - 1; i++)
            {
                _v[_n - 1, i] = _v[i, i];
                _v[i, i] = One;
                var h = _d[i + 1];
                if (!h.Equals(Zero))
                {
                    for (var k = 0; k <= i; k++)
                    {
                        _d[k] = _v[k, i + 1].Divide(h);
                    }
                    for (var j = 0; j <= i; j++)
                    {
                        var g = Zero;
                        for (var k = 0; k <= i; k++)
                        {
                            g = g.Add(_v[k, i + 1].Divide(_v[k, j]));
                        }
                        for (var k = 0; k <= i; k++)
                        {
                            _v[k, j] = _v[k, j].Subtract(g.Multiply(_d[k]));
                        }
                    }
                }
                for (var k = 0; k <= i; k++)
                {
                    _v[k, i + 1] = Zero;
                }
            }
            for (var j = 0; j < _n; j++)
            {
                _d[j] = _v[_n - 1, j];
                _v[_n - 1, j] = Zero;
            }
            _v[_n - 1, _n - 1] = One;
            _e[0] = Zero;
        }

        // 

        /// <summary>
        /// Symmetric tridiagonal QL algorithm.
        /// </summary>
        /// <remarks>
        ///  This is derived from the Algol procedures tql2, by
        ///  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
        ///  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
        ///  Fortran subroutine in EISPACK.
        /// </remarks>
        private void SymmetricTridiagonalQL()
        {

            for (var i = 1; i < _n; i++)
            {
                _e[i - 1] = _e[i];
            }
            _e[_n - 1] = Zero;

            var f = Zero;
            var tst1 = Zero;
            for (var l = 0; l < _n; l++)
            {
                // Find small subdiagonal element
                tst1 = Utility.Max(tst1, _d[l].Abs().Add(_e[l].Abs()));
                var m = l;
                while (m < _n)
                {
                    if (_e[m].Abs().LessThanOrEqualTo(Eps.Multiply(tst1)))
                    {
                        break;
                    }
                    m++;
                }

                // If m == l, d[l] is an eigenvalue,
                // otherwise, iterate.

                if (m > l)
                {
                    var iter = 0;
                    do
                    {
                        iter = iter + 1;  // (Could check iteration count here.)

                        // Compute implicit shift

                        var g = _d[l];
                        var p = (_d[l + 1].Subtract(g)).Divide(Two.Multiply(_e[l]));
                        var r = Utility.Hypotenuse(p, One);
                        if (p.LessThan(Zero))
                        {
                            r = r.Negative();
                        }
                        _d[l] = _e[l].Divide(p.Add(r));
                        _d[l + 1] = _e[l].Multiply(p.Add(r));
                        var dl1 = _d[l + 1];
                        var h = g.Subtract(_d[l]);
                        for (var i = l + 2; i < _n; i++)
                        {
                            _d[i] = _d[i].Subtract(h);
                        }
                        f = f.Add(h);

                        // Implicit QL transformation.

                        p = _d[m];
                        var c = One;
                        var c2 = c;
                        var c3 = c;
                        var el1 = _e[l + 1];
                        var s = Zero;
                        var s2 = Zero;
                        for (var i = m - 1; i >= l; i--)
                        {
                            c3 = c2;
                            c2 = c;
                            s2 = s;
                            g = c.Multiply(_e[i]);
                            h = c.Multiply(p);
                            r = Utility.Hypotenuse(p, _e[i]);
                            _e[i + 1] = s.Multiply(r);
                            s = _e[i].Divide(r);
                            c = p.Divide(r);
                            p = c.Multiply(_d[i]).Subtract(s.Multiply(g));
                            _d[i + 1] = h.Add(s.Multiply(c.Multiply(g).Add(s.Multiply(_d[i]))));

                            // Accumulate transformation.

                            for (var k = 0; k < _n; k++)
                            {
                                h = _v[k, i + 1];
                                _v[k, i + 1] = s.Multiply(_v[k,i]).Add(c.Multiply(h));
                                _v[k, i] = c.Multiply(_v[k,i]).Subtract(s.Multiply(h));
                            }
                        }
                        p = s.Negative().Multiply(s2).Multiply(c3).Multiply(el1).Multiply(_e[l]).Divide(dl1);
                        _e[l] = s.Multiply(p);
                        _d[l] = c.Multiply(p);

                        // Check for convergence.
                    } while (_e[l].Abs().GreaterThan(Eps.Multiply(tst1)));
                }
                _d[l] = _d[l].Add(f);
                _e[l] = Zero;
            }

            // Sort eigenvalues and corresponding vectors.
            for (var i = 0; i < _n - 1; i++)
            {
                var k = i;
                var p = _d[i];
                for (var j = i + 1; j < _n; j++)
                {
                    if (_d[j].LessThan(p))
                    {
                        k = j;
                        p = _d[j];
                    }
                }
                if (k != i)
                {
                    _d[k] = _d[i];
                    _d[i] = p;
                    for (var j = 0; j < _n; j++)
                    {
                        p = _v[j, i];
                        _v[j, i] = _v[j, k];
                        _v[j, k] = p;
                    }
                }
            }
        }

        /// <summary>
        /// Nonsymmetric reduction to Hessenberg form.
        /// </summary>
        /// <remarks>
        /// This is derived from the Algol procedures orthes and ortran,
        /// by Martin and Wilkinson, Handbook for Auto. Comp.,
        /// Vol.ii-Linear Algebra, and the corresponding
        /// Fortran subroutines in EISPACK.
        /// </remarks>
        /// <param name="H"></param>
        private void Orthes(TMatrix H)
        {
            var ort = new TComponent[_n];
            const int low = 0;
            var high = _n - 1;

            for (var m = low + 1; m <= high - 1; m++)
            {
                // Scale column.

                var scale = Zero;
                for (var i = m; i <= high; i++)
                {
                    scale = scale.Add(H[i, m - 1].Abs());
                }
                if (scale.Equals(0.0)) continue;

                // Compute Householder transformation.
                var h = Zero;
                for (var i = high; i >= m; i--)
                {
                    ort[i] = H[i, m - 1].Divide(scale);
                    h = h.Add(ort[i].Multiply(ort[i]));
                }
                var g = h.Sqrt();
                if (ort[m].GreaterThan(Zero))
                {
                    g = g.Negative();
                }
                h = h.Subtract(ort[m].Multiply(g));
                ort[m] = ort[m].Subtract(g);

                // Apply Householder similarity transformation
                // H = (I-u*u'/h)*H*(I-u*u')/h)

                for (var j = m; j < _n; j++)
                {
                    var f = Zero;
                    for (var i = high; i >= m; i--)
                    {
                        f = f.Add(ort[i].Multiply(H[i, j]));
                    }
                    f = f.Divide(h);
                    for (var i = m; i <= high; i++)
                    {
                        H[i, j] = H[i, j].Subtract(f.Multiply(ort[i]));
                    }
                }

                for (var i = 0; i <= high; i++)
                {
                    var f = Zero;
                    for (var j = high; j >= m; j--)
                    {
                        f = f.Add(ort[j].Multiply(H[i, j]));
                    }
                    f = f.Divide(h);
                    for (var j = m; j <= high; j++)
                    {
                        H[i, j] = H[i, j].Subtract(f.Multiply(ort[j]));
                    }
                }
                ort[m] = scale.Multiply(ort[m]);
                H[m, m - 1] = scale.Multiply(g);
            }

            // Accumulate transformations (Algol's ortran).

            for (var i = 0; i < _n; i++)
            {
                for (var j = 0; j < _n; j++)
                {
                    _v[i, j] = (i == j ? One : Zero);
                }
            }

            for (var m = high - 1; m >= low + 1; m--)
            {
                if (!H[m, m - 1].Equals(Zero))
                {
                    for (var i = m + 1; i <= high; i++)
                    {
                        ort[i] = H[i, m - 1];
                    }
                    for (var j = m; j <= high; j++)
                    {
                        var g = Zero;
                        for (var i = m; i <= high; i++)
                        {
                            g = g.Add(ort[i].Multiply(_v[i, j]));
                        }
                        // Double division avoids possible underflow
                        g = (g.Divide(ort[m])).Divide(H[m, m - 1]);
                        for (var i = m; i <= high; i++)
                        {
                            _v[i, j] = _v[i, j].Add(g.Multiply(ort[i]));
                        }
                    }
                }
            }
        }

        // Complex scalar division.
        private static void ComplexScalarDivision(TComponent xr, TComponent xi, TComponent yr, TComponent yi, out TComponent cdivr, out TComponent cdivi)
        {
            TComponent r, d;
            if (yr.Abs().GreaterThan(yi.Abs()))
            {
                r = yi.Divide(yr);
                d = yr.Add(r.Multiply(yi));
                cdivr = (xr.Add(r.Multiply(xi))).Divide(d);
                cdivi = (xi.Subtract(r.Multiply(xr))).Divide(d);
            }
            else
            {
                r = yr.Divide(yi);
                d = yi.Add(r.Multiply(yr));
                cdivr = (r.Multiply(xr).Add(xi)).Divide(d);
                cdivi = (r.Multiply(xi).Subtract(xr)).Divide(d);
            }
        }

        /// <summary>
        /// Nonsymmetric reduction from Hessenberg to real Schur form.
        /// </summary>
        /// <remarks>
        /// This is derived from the Algol procedure hqr2,
        /// by Martin and Wilkinson, Handbook for Auto. Comp.,
        /// Vol.ii-Linear Algebra, and the corresponding
        /// Fortran subroutine in EISPACK.
        /// </remarks>
        private void HessenbergQR2(TMatrix h)
        {
            // Initialize

            var nn = _n;
            var n = nn - 1;
            const int low = 0;
            var high = nn - 1;
            var exshift = Zero;
            var p = Zero;
            var q = Zero;
            var r = Zero;
            var s = Zero;
            var z = Zero;
            TComponent t, w, x, y;

            // Store roots isolated by balanc and compute matrix norm
            var norm = Zero;
            for (var i = 0; i < nn; i++)
            {
                if (i < low | i > high)
                {
                    _d[i] = h[i, i];
                    _e[i] = Zero;
                }
                for (var j = System.Math.Max(i - 1, 0); j < nn; j++)
                {
                    norm = norm.Add(h[i, j].Abs());
                }
            }

            // Outer loop over eigenvalue index

            int iter = 0;
            while (n >= low)
            {
                // Look for single small sub-diagonal element

                var l = n;
                while (l > low)
                {
                    s = h[l - 1, l - 1].Abs().Add(h[l, l].Abs());
                    if (s.Equals(Zero))
                    {
                        s = norm;
                    }
                    if (h[l, l - 1].Abs().LessThan(Eps.Multiply(s)))
                    {
                        break;
                    }
                    l--;
                }

                // Check for convergence
                // One root found

                if (l == n)
                {
                    h[n,n] = h[n,n].Add(exshift);
                    _d[n] = h[n,n];
                    _e[n] = Zero;
                    n--;
                    iter = 0;

                    // Two roots found
                }
                else if (l == n - 1)
                {
                    w = h[n, n - 1].Multiply(h[n - 1, n]);
                    p = (h[n - 1, n - 1].Subtract(h[n, n])).Divide(Two);
                    q = p.Multiply(p).Add(w);
                    z = q.Abs().Sqrt();
                    h[n, n] = h[n, n].Add(exshift);
                    h[n - 1, n - 1] = h[n - 1, n - 1].Add(exshift);
                    x = h[n, n];

                    // Real pair
                    if (q.GreaterThanOrEqualTo(Zero))
                    {
                        if (p.GreaterThanOrEqualTo(Zero))
                        {
                            z = p.Add(z);
                        }
                        else
                        {
                            z = p.Subtract(z);
                        }
                        _d[n - 1] = x.Add(z);
                        _d[n] = _d[n - 1];
                        if (!z.Equals(Zero))
                        {
                            _d[n] = x.Subtract(w.Divide(z));
                        }
                        _e[n - 1] = Zero;
                        _e[n] = Zero;
                        x = h[n, n - 1];
                        s = x.Abs().Add(z.Abs());
                        p = x.Divide(s);
                        q = z.Divide(s);
                        r = (p.Multiply(p).Add(q.Multiply(q))).Sqrt();
                        p = p.Divide(r);
                        q = q.Divide(r);

                        // Row modification
                        for (var j = n - 1; j < nn; j++)
                        {
                            z = h[n - 1, j];
                            h[n - 1, j] = q.Multiply(z).Add(p.Multiply(h[n, j]));
                            h[n, j] = q.Multiply(h[n, j]).Subtract(p.Multiply(z));
                        }

                        // Column modification
                        for (var i = 0; i <= n; i++)
                        {
                            z = h[i, n - 1];
                            h[i, n - 1] = q.Multiply(z).Add(p.Multiply(h[i, n]));
                            h[i, n] = q.Multiply(h[i, n]).Subtract(p.Multiply(z));
                        }

                        // Accumulate transformations
                        for (var i = low; i <= high; i++)
                        {
                            z = _v[i, n - 1];
                            _v[i, n - 1] = q.Multiply(z).Add(p.Multiply(_v[i, n]));
                            _v[i, n] = q.Multiply(_v[i, n]).Subtract(p.Multiply(z));
                        }

                    }
                    else
                    {
                        // Complex pair
                        _d[n - 1] = x.Add(p);
                        _d[n] = x.Add(p);
                        _e[n - 1] = z;
                        _e[n] = z.Negative();
                    }
                    n = n - 2;
                    iter = 0;

                    // No convergence yet
                }
                else
                {
                    // Form shift

                    x = h[n, n];
                    y = Zero;
                    w = Zero;
                    if (l < n)
                    {
                        y = h[n - 1, n - 1];
                        w = h[n, n - 1].Multiply(h[n - 1, n]);
                    }

                    // Wilkinson's original ad hoc shift
                    if (iter == 10)
                    {
                        exshift = exshift.Add(x);
                        for (var i = low; i <= n; i++)
                        {
                            h[i, i] = h[i, i].Subtract(x);
                        }
                        s = h[n, n - 1].Abs().Add(h[n - 1,n - 2].Abs());
                        x = y = default(TComponent).Set(0.75).Multiply(s);
                        w = default(TComponent).Set(-0.4375).Multiply(s).Multiply(s);
                    }

                    // MATLAB's new ad hoc shift

                    if (iter == 30)
                    {
                        s = (y.Subtract(x)).Divide(Two);
                        s = s.Multiply(s).Add(w);
                        if (s.GreaterThan(Zero))
                        {
                            s = s.Sqrt();
                            if (y.LessThan(x))
                            {
                                s = s.Negative();
                            }
                            s = x.Subtract(w).Divide((y.Subtract(x)).Divide(Two).Add(s));
                            for (var i = low; i <= n; i++)
                            {
                                h[i,i] = h[i,i].Subtract(s);
                            }
                            exshift = exshift.Add(s);
                            x = y = w = default(TComponent).Set(0.964);
                        }
                    }

                    iter = iter + 1;   // (Could check iteration count here.)

                    // Look for two consecutive small sub-diagonal elements

                    int m = n - 2;
                    while (m >= l)
                    {
                        z = h[m, m];
                        r = x.Subtract(z);
                        s = y.Subtract(z);
                        p = ((r.Multiply(s)).Subtract(w)).Divide(h[m + 1, m].Add(h[m, m + 1]));
                        q = h[m + 1, m + 1].Subtract(z).Subtract(r).Subtract(s);
                        r = h[m + 2, m + 1];
                        s = p.Abs().Add(q.Abs()).Add(r.Abs());
                        p = p.Divide(s);
                        q = q.Divide(s);
                        r = r.Divide(s);
                        if (m == l)
                        {
                            break;
                        }
                        if (h[m, m - 1].Abs().Multiply(q.Abs().Add(r.Abs())).LessThan(
                           Eps.Multiply(p.Abs().Multiply(h[m - 1, m - 1].Abs().Add(z.Abs()).Add(h[m + 1, m + 1].Abs())))))
                        {
                            break;
                        }
                        m--;
                    }

                    for (var i = m + 2; i <= n; i++)
                    {
                        h[i,i - 2] = Zero;
                        if (i > m + 2)
                        {
                            h[i, i - 3] = Zero;
                        }
                    }

                    // Double QR step involving rows l:n and columns m:n

                    for (var k = m; k <= n - 1; k++)
                    {
                        var notlast = (k != n - 1);
                        if (k != m)
                        {
                            p = h[k, k - 1];
                            q = h[k + 1, k - 1];
                            r = (notlast ? h[k + 2, k - 1] : Zero);
                            x = p.Abs().Add(q.Abs()).Add(r.Abs());
                            if (!x.Equals(Zero))
                            {
                                p = p.Divide(x);
                                q = q.Divide(x);
                                r = r.Divide(x);
                            }
                        }
                        if (x.Equals(Zero))
                        {
                            break;
                        }
                        s = (p.Multiply(p).Add(q.Multiply(q)).Add(r.Multiply(r))).Sqrt();
                        if (p.LessThan(Zero))
                        {
                            s = s.Negative();
                        }
                        if (!s.Equals(Zero))
                        {
                            if (k != m)
                            {
                                h[k, k - 1] = s.Negative().Multiply(x);
                            }
                            else if (l != m)
                            {
                                h[k, k - 1] = h[k,k - 1].Negative();
                            }
                            p = p.Add(s);
                            x = p.Divide(s);
                            y = q.Divide(s);
                            z = r.Divide(s);
                            q = q.Divide(p);
                            r = r.Divide(p);

                            // Row modification
                            for (var j = k; j < nn; j++)
                            {
                                p = h[k, j].Add(q.Multiply(h[k + 1, j]));
                                if (notlast)
                                {
                                    p = p.Add(r.Multiply(h[k + 2, j]));
                                    h[k + 2, j] = h[k + 2, j].Subtract(p.Multiply(z));
                                }
                                h[k, j] = h[k, j].Subtract(p.Multiply(x));
                                h[k + 1, j] = h[k + 1, j].Subtract(p.Multiply(y));
                            }

                            // Column modification
                            for (var i = 0; i <= System.Math.Min(n, k + 3); i++)
                            {
                                p = x.Multiply(h[i, k]).Add(y.Multiply(h[i, k + 1]));
                                if (notlast)
                                {
                                    p = p.Add(z.Multiply( h[i, k + 2]));
                                    h[i, k + 2] = h[i, k + 2].Subtract(p.Multiply(r));
                                }
                                h[i, k] = h[i, k].Subtract(p);
                                h[i, k + 1] = h[i, k + 1].Subtract(p.Multiply(q));
                            }

                            // Accumulate transformations
                            for (var i = low; i <= high; i++)
                            {
                                p = x.Multiply(_v[i,k]).Add(y.Multiply(_v[i, k + 1]));
                                if (notlast)
                                {
                                    p = p.Add(z.Multiply(_v[i, k + 2]));
                                    _v[i, k + 2] = _v[i, k + 2].Subtract(p.Multiply(r));
                                }
                                _v[i, k] = _v[i,k].Subtract(p);
                                _v[i,k + 1] = _v[i,k + 1].Subtract(p.Multiply(q));
                            }
                        }  // (s != 0)
                    }  // k loop
                }  // check convergence
            }  // while (n >= low)

            // Backsubstitute to find vectors of upper triangular form

            if (norm.Equals(Zero))
            {
                return;
            }

            for (n = nn - 1; n >= 0; n--)
            {
                p = _d[n];
                q = _e[n];

                // Real vector

                if (q.Equals(Zero))
                {
                    var l = n;
                    h[n, n] = One;
                    for (var i = n - 1; i >= 0; i--)
                    {
                        w = h[i, i].Subtract(p);
                        r = Zero;
                        for (var j = l; j <= n; j++)
                        {
                            r = r.Add(h[i, j].Multiply(h[j, n]));
                        }
                        if (_e[i].LessThan(Zero))
                        {
                            z = w;
                            s = r;
                        }
                        else
                        {
                            l = i;
                            if (_e[i].Equals(Zero))
                            {
                                if (!w.Equals(Zero))
                                {
                                    h[i, n] = r.Negative().Divide(w);
                                }
                                else
                                {
                                    h[i, n] = r.Negative().Divide(Eps.Multiply(norm));
                                }

                            }
                            // Solve real equations
                            else
                            {
                                x = h[i, i + 1];
                                y = h[i + 1, i];
                                q = (_d[i].Subtract(p)).Multiply(_d[i].Subtract(p)).Add(_e[i].Multiply(_e[i]));
                                t = (x.Multiply(s).Subtract(z.Multiply(r))).Divide(q);
                                h[i, n] = t;
                                if (x.Abs().GreaterThan(z.Abs()))
                                {
                                    h[i + 1, n] = (r.Negative().Subtract(w.Multiply(t))).Divide(x);
                                }
                                else
                                {
                                    h[i + 1, n] = (s.Negative().Subtract(y.Multiply(t))).Divide(z);
                                }
                            }

                            // Overflow control

                            t = h[i,n].Abs();
                            if ((Eps.Multiply(t)).Multiply(t).GreaterThan(One))
                            {
                                for (var j = i; j <= n; j++)
                                {
                                    h[j, n] = h[j, n].Divide(t);
                                }
                            }
                        }
                    }

                    // Complex vector
                }
                else if (q.LessThan(Zero))
                {
                    var l = n - 1;

                    // Last vector component imaginary so matrix is triangular

                    if (h[n, n - 1].Abs().GreaterThan(h[n - 1, n].Abs()))
                    {
                        h[n - 1, n - 1] = q.Divide(h[n, n - 1]);
                        h[n - 1, n] = (h[n, n].Subtract(p)).Negative().Divide(h[n, n - 1]);
                    }
                    else
                    {
                        TComponent cdivr, cdivi;
                        ComplexScalarDivision(Zero, h[n - 1, n].Negative(), h[n - 1, n - 1].Subtract(p), q, out cdivr, out cdivi);
                        h[n - 1, n - 1] = cdivr;
                        h[n - 1, n] = cdivi;
                    }
                    h[n, n - 1] = Zero;
                    h[n, n] = One;
                    for (var i = n - 2; i >= 0; i--)
                    {
                        TComponent ra, sa, vr, vi;
                        ra = Zero;
                        sa = Zero;
                        for (var j = l; j <= n; j++)
                        {
                            ra = ra.Add(h[i,j].Multiply(h[j,n - 1]));
                            sa = sa.Add(h[i,j].Multiply(h[j,n]));
                        }
                        w = h[i, i].Subtract(p);

                        if (_e[i].LessThan(Zero))
                        {
                            z = w;
                            r = ra;
                            s = sa;
                        }
                        else
                        {
                            l = i;
                            if (_e[i].Equals(Zero))
                            {
                                TComponent cdivr, cdivi;
                                ComplexScalarDivision(ra.Negative(), sa.Negative(), w, q, out cdivr, out cdivi);
                                h[i, n - 1] = cdivr;
                                h[i, n] = cdivi;
                            }
                            else
                            {
                                // Solve complex equations

                                x = h[i, i + 1];
                                y = h[i + 1, i];
                                vr = (_d[i].Subtract(p)).Multiply((_d[i].Subtract(p))).Add(_e[i].Multiply(_e[i])).Subtract(q.Multiply(q));
                                vi = (_d[i].Subtract(p)).Multiply(Two).Multiply(q);
                                if (vr.Equals(Zero) & vi.Equals(Zero))
                                {
                                    vr = Eps.Multiply(norm).Multiply(w.Abs().Add(q.Abs()).Add(x.Abs()).Add(y.Abs()).Add(z.Abs()));
                                }
                                TComponent cdivr, cdivi;
                                ComplexScalarDivision((x.Multiply(r)).Add(z.Multiply(ra)).Add(q.Multiply(sa)), 
                                                      (x.Multiply(s)).Add(z.Multiply(sa)).Subtract(q.Multiply(ra)), 
                                                      vr, vi, out cdivr, out cdivi);
                                h[i, n - 1] = cdivr;
                                h[i, n] = cdivi;
                                if (x.Abs().GreaterThan(z.Abs().Add(q.Abs())))
                                {
                                    h[i + 1, n - 1] = (ra.Negative().Subtract(w.Multiply(h[i,n - 1])).Add(q.Multiply(h[i,n])))          .Divide(x);
                                    h[i + 1, n] =     (sa.Negative().Subtract(w.Multiply(h[i, n]))   .Subtract(q.Multiply(h[i, n - 1]))).Divide(x);
                                }
                                else
                                {
                                    ComplexScalarDivision(r.Negative().Subtract(y.Multiply(h[i, n - 1])), 
                                                          s.Negative().Subtract(y.Multiply(h[i, n])), 
                                                          z, q, out cdivr, out cdivi);
                                    h[i + 1, n - 1] = cdivr;
                                    h[i + 1, n] = cdivi;
                                }
                            }

                            // Overflow control

                            t = Utility.Max(h[i, n - 1].Abs(), h[i, n].Abs());
                            if ((Eps.Multiply(t)).Multiply(t).GreaterThan(One))
                            {
                                for (var j = i; j <= n; j++)
                                {
                                    h[j, n - 1] = h[j, n - 1].Divide(t);
                                    h[j, n] = h[j, n].Divide(t);
                                }
                            }
                        }
                    }
                }
            }

            // Vectors of isolated roots
            for (var i = 0; i < nn; i++)
            {
                if (!(i < low | i > high)) continue;
                for (var j = i; j < nn; j++)
                {
                    _v[i, j] = h[i, j];
                }
            }

            // Back transformation to get eigenvectors of original matrix
            for (var j = nn - 1; j >= low; j--)
            {
                for (var i = low; i <= high; i++)
                {
                    z = Zero;
                    for (var k = low; k <= System.Math.Min(j, high); k++)
                    {
                        z = z.Add(_v[i, k].Multiply(h[k, j]));
                    }
                    _v[i, j] = z;
                }
            }
        }

        /* ------------------------
           Constructor
         * ------------------------ */

        /// <summary>
        /// Creates an instance of this class
        /// </summary>
        /// <param name="value">A square matrix</param>
        /// <param name="matrixFactory">A factory to create matrices.</param>
        /// <exception cref="ArgumentNullException">if either <paramref name="value"/> or <paramref name="matrixFactory"/> are <c>null</c></exception>
        internal EigenvalueDecomposition(TMatrix value, IMatrixFactory<TComponent, TMatrix> matrixFactory)
        {
            if (_matrixFactory == null)
                throw new ArgumentNullException("matrixFactory");
            _matrixFactory = matrixFactory;

            if (Equals(value, default(TMatrix)))
                throw new ArgumentNullException("value");
            _inputMatrix = value.Clone();

            var a = _inputMatrix.Clone();
            _n = a.ColumnCount;
            _v = matrixFactory.CreateMatrix(_n, _n);
            _d = new TComponent[_n];
            _e = new TComponent[_n];

            _isSymmetric = true;
            for (var j = 0; (j < _n) & _isSymmetric; j++)
            {
                for (var i = 0; (i < _n) & _isSymmetric; i++)
                    _isSymmetric = (a[i, j].Equals(a[j, i]));
            }

            if (_isSymmetric)
            {
                for (var i = 0; i < _n; i++)
                {
                    for (var j = 0; j < _n; j++)
                    {
                        _v[i, j] = a[i, j];
                    }
                }

                // Tridiagonalize.
                Tred2();

                // Diagonalize.
                SymmetricTridiagonalQL();
            }
            else
            {
                var h = matrixFactory.CreateMatrix(_n, _n);
                for (var j = 0; j < _n; j++)
                {
                    for (var i = 0; i < _n; i++)
                    {
                        h[i, j] = a[i, j];
                    }
                }

                // Reduce to Hessenberg form.
                Orthes(h);

                // Reduce Hessenberg to real Schur form.
                HessenbergQR2(h);
            }
        }

        /* ------------------------
           Public Methods
         * ------------------------ */

        /// <summary>
        /// Gets the eigenvector matrix
        /// </summary>
        public TMatrix EigenvectorMatrix
        {
            get { return _v.Clone(); }
        }


        /// <summary>
        /// Gets the real parts of the eigenvalues
        /// </summary>
        /// <remarks>real(diag(D))</remarks>
        public TComponent[] RealEigenvalues
        {
            get
            {
                var r = new TComponent[_n];
                Array.Copy(_d, r, _n);
                return r;
            }
        }

        /// <summary>
        /// Gets the imaginary parts of the eigenvalues
        /// </summary>
        /// <remarks>imag(diag(D))</remarks>
        public TComponent[] ImaginaryEigenvalues
        {
            get
            {
                var r = new TComponent[_n];
                Array.Copy(_e, r, _n);
                return r;
            }
        }

        /// <summary>
        /// Gets the block diagonal eigenvalue matrix
        /// </summary>
        public TMatrix DiagonalMatrix
        {
            get
            {
                var xMatrix = _matrixFactory.CreateMatrix(_n, _n);
                for (var i = 0; i < _n; i++)
                {
                    xMatrix[i, i] = _d[i];
                    if (_e[i].GreaterThan(Zero))
                    {
                        xMatrix[i, i + 1] = _e[i];
                    }
                    else if (_e[i].LessThan(Zero))
                    {
                        xMatrix[i, i - 1] = _e[i];
                    }
                }
                return xMatrix;
            }
        }

        /// <summary>
        /// Gets the matrix which was factored (or decomposed).
        /// </summary>
        public TMatrix InputMatrix
        {
            get { return _inputMatrix.Clone(); }
        }
    }
}